#include "WP.h"
#include <Math.h>

#define DEG2RAD 0.0174532925
#define RAD2DEG 57.2957795

//WP::nextId = 1; //@TODO: FIX THIS PLEASE

LWP::LWP(double lat, double lon, double alt, double heading, double gs)
{
	maked(lat,lon,alt);
	setHeading(heading);
	setGlideSlope(gs);
}

void LWP::setHeading(double heading){landHeading = heading;}
void LWP::setGlideSlope(double gs){glideSlope = gs;}

double LWP::getHeading(){return landHeading;}
double LWP::getGlideSlope(){return glideSlope;}

WP::WP()
{
	lat = lon = alt = 0.0;
	id = nextId;
	nextId++;
}
WP::WP(double dlat, double dlon, double alt)
{
	maked(dlat,dlon,alt);
}
	
void WP::maked(double dlat, double dlon, double alt)
{
	setDcoord(dlat,dlon);
  setAlt(alt);
}
void WP::maker(double rlat, double rlon, double alt)
{
	setRcoord(rlat,rlon);
  setAlt(alt);
}

double WP::getAlt() const{return alt;}
unsigned int WP::getId() const{return id;}

void WP::setDcoord(double dlat, double dlon){setLatd(dlat);setLond(dlat);}
void WP::setRcoord(double rlat, double rlon){setLatr(rlat);setLonr(rlon);}
void WP::setLatd(double dlat){lat = dlat * DEG2RAD;}
void WP::setLond(double dlon){lon = dlon * DEG2RAD;}
void WP::setLatr(double rlat){lat = rlat;}
void WP::setLonr(double rlon){lon = rlon;}
void WP::setAlt(double alt){this->alt = alt;}

//Generation
double WP::dist3D(const WP &wp, double unit) const
{
	double d = dist(wp,unit);
	double h = alt - wp.getAlt();
	if (h<0){h*=-1;}
	h *= unit/UNIT_FEET;
	return sqrt((d*d)+(h*h));
}

double WP::thetaD(const WP &wp) const
{
	return thetaR(wp)*RAD2DEG;
}

double WP::thetaR(const WP &wp) const
{
	double d,h;
	d = dist(wp,UNIT_FEET);
	h = alt - wp.getAlt();
	if (h<0){h*=-1;}
	return atan2(h,d);
}
